#include "ConverterMMM2Robot.h"
#include "ConverterMMM2RobotFactory.h"
#include <VirtualRobot/MathTools.h>
#include <MMM/RapidXML/rapidxml.hpp>
#include <MMMSimoxTools/MMMSimoxTools.h>

#include <iostream>
#include <string>

using std::cout;
using std::endl;
using namespace VirtualRobot;

namespace MMM
{

ConverterMMM2Robot::ConverterMMM2Robot(const std::string &name)
    : MarkerBasedConverter(name)
{
    std::srand(static_cast<unsigned int>(std::time(0)));
    paramIKStepSize = 0.6f;//0.17f;//0.03f;
    paramIKMinChange = 0.0f;
    paramIKSteps = 10;
    paramCheckImprovement = false;
    paramPerfomMinOneStep = false;
    paramJointLimitsBoxConstraints = true;
    paramInitialIKStepSize = 0.8f;//0.17f;//0.03f;
    paramInitialIKMinChange = 0.0f;
    paramInitialIKSteps = 50; //50;
    paramInitialCheckImprovement = false;
    paramInitialPerfomMinOneStep = false;
    paramInitialJointLimitsBoxConstraints = true;
    markerMotion.reset(new MMM::MarkerMotion());
}

bool ConverterMMM2Robot::setup(ModelPtr sourceModel, AbstractMotionPtr motionData, ModelPtr targetModel)
{
    cout << "ConverterMMM2Robot... setup() started!" << endl;
    if (!sourceModel || !motionData || !targetModel)
		return false;
    _motion = boost::dynamic_pointer_cast<MMM::LegacyMotion>(motionData);
    if (!_motion)
        return false;
    // build robot constructs from model constructs
    buildModel(sourceModel, targetModel);
    if (!sourceRobot || !targetRobot)
    {
        cout << "building robot constructs from models failed!" << endl;
        return false;
    }

    /*
    // build simox robot from source model (no need for viusalization)
    robot = MMM::SimoxTools::buildModel(sourceModel, false);
    // create the robotNodeSet
    std::vector<std::string> jointNames = motion->getJointNames();
    // match nodes to jointNames
    std::vector<RobotNodePtr> nodes;
    for (size_t i = 0; i < jointNames.size(); i++)
        nodes.push_back(robot->getRobotNode(jointNames[i]));
    VirtualRobot::RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, "ConverterMMM2Robot_Joints", nodes, robot->getRootNode(), VirtualRobot::RobotNodePtr(), true);
    */


    // * extract virtual markers from mmm model (sourceModel)
    std::vector<SensorPtr> sensors = sourceRobot->getSensors();
    std::vector<std::string> sensorList;
    // extract sensor names
    for (size_t i = 0; i < sensors.size(); i++)
        sensorList.push_back(sensors[i]->getName());
    markerMotion->setMarkerLabels(sensorList);
    markerMotion->setNumFrames(_motion->getNumFrames());
    // for every frame set the joint configuration to the mmm model, extract sensor positions and save them as markerMotion
    for (size_t i = 0; i < _motion->getNumFrames(); i++)
    {
        MMM::LegacyMarkerDataPtr md = markerMotion->getFrame(i);
        MMM::MotionFramePtr mf = _motion->getMotionFrame(i);
        md->frame = i;
        //md->timestamp = mf->timestep;
        transformPoseToMarker(mf, md);
    }
    // save this markerMotion as input motion for the conversion
    inputMarkerMotion = markerMotion;
    cout << "ConverterMMM2Robot... setup() done!" << endl;
    return Converter::setup(sourceModel, markerMotion, targetModel);
}

bool ConverterMMM2Robot::transformPoseToMarker(MMM::MotionFramePtr mf_input, MMM::LegacyMarkerDataPtr md_output)
{
    float scale = mTargetModelSize / mSourceModelSize;
    if (!mf_input || !md_output)
    {
        MMM_INFO << "ConverterMMM2Robot::transformPoseToMarker: no valid motionFrame or MotionData!" << std::endl;
        return false;
    }
    if (!sourceRobot || !rnsSource)
    {
        MMM_INFO << "ConverterMMM2Robot::transformPoseToMarker: no source Robot or source RobotNodeSet defined!" << std::endl;
        return false;
    }
    std::vector<SensorPtr> sensors = sourceRobot->getSensors();
    // set robot pose, according to MotionFrame mf
    sourceRobot->setGlobalPose(mf_input->getRootPose()); // sourceRobot = MMM Modell
    rnsSource->setJointValues(mf_input->joint);
    // now extract sensor positions
    for (size_t j = 0; j < sensors.size(); j++)
    {
        Eigen::Matrix4f pose = sensors[j]->getGlobalPose();
        Eigen::Vector3f pos = pose.block(0, 3, 3, 1) * scale;
        // save sensor position in LegacyMarkerData object
        md_output->data[sensors[j]->getName()]=pos;
    }
    md_output->timestamp = mf_input->timestep;
    return true;
}

bool ConverterMMM2Robot::buildModel(MMM::ModelPtr source, MMM::ModelPtr target)
{
    // build and initialise source model (MMM reference model)
    sourceRobot = MMM::SimoxTools::buildModel(source, false);
    if (!sourceRobot)
    {
        MMM_ERROR << "could not read source model file (MMM reference model)!" << endl;
        return false;
    }
    // update RNS
    if (sourceRobot->hasRobotNodeSet("ConverterMMM2Robot"))
        sourceRobot->deregisterRobotNodeSet(sourceRobot->getRobotNodeSet("ConverterMMM2Robot"));
    rnsSource.reset();
    // set the jointOrder according to motion file
    jointOrderSource = _motion->getJointNames();
    // find joints in robot according to joint names
    if (jointOrderSource.size()>0)
    {
        std::vector<RobotNodePtr> nodes;
    //    nodes.push_back(mmmModel->getRootNode());
        for (size_t i = 0; i < jointOrderSource.size(); i++)
        {
            if (sourceRobot->hasRobotNode(jointOrderSource[i]))
            {
                nodes.push_back(sourceRobot->getRobotNode(jointOrderSource[i]));
            }
            else
            {
                cout << "Skipping invalid joint name in converter config (source model):" << jointOrderSource[i] << endl;
            }
        }
        rnsSource = RobotNodeSet::createRobotNodeSet(sourceRobot, "ConverterMMM2Robot", nodes, sourceRobot->getRootNode(), VirtualRobot::RobotNodePtr(), true);
    } else
        MMM_ERROR << "Could not initialize ConverterMMM2Robot correctly due to missing joint definitions (source model)" << endl;
	// store markers
    std::vector<SensorPtr> sensorsSrc = sourceRobot->getSensors();
    for (size_t i = 0; i < sensorsSrc.size(); i++)
	{
        sourceMarkers[sensorsSrc[i]->getName()] = sensorsSrc[i];
	}
    if (modelNameSrc.empty())
        sourceRobot->setName(sourceRobot->getType());
	else
        sourceRobot->setName(modelNameSrc);
    // build and initialise target model (robot model)
    targetRobot = MMM::SimoxTools::buildModel(target, false);
    if (!targetRobot)
    {
        MMM_ERROR << "could not read target model file (robot model)!" << endl;
        return false;
    }
    // update RNS
    if (targetRobot->hasRobotNodeSet("ConverterMMM2Robot"))
        targetRobot->deregisterRobotNodeSet(targetRobot->getRobotNodeSet("ConverterMMM2Robot"));
    rnsTarget.reset();
/*
    // todo: set the jointOrder according to config file
    jointOrder

    std::vector<VirtualRobot::RobotNodePtr> nodeList;
    targetRobot->getRobotNodes(nodeList);
    // save node names
    for (size_t i=0; i < nodeList.size(); i++)
        jointOrder.push_back(nodeList[i]->getName());
*/
    if (jointOrder.size()>0)
    {
        std::vector<RobotNodePtr> nodes;
    //    nodes.push_back(mmmModel->getRootNode());
        for (size_t i = 0; i < jointOrder.size(); i++)
        {
            if (targetRobot->hasRobotNode(jointOrder[i]))
            {
                nodes.push_back(targetRobot->getRobotNode(jointOrder[i]));
            }
            else
            {
                cout << "Skipping invalid joint name in converter config  (target model):" << jointOrder[i] << endl;
            }
        }

        rnsTarget = RobotNodeSet::createRobotNodeSet(targetRobot, "ConverterMMM2Robot", nodes, targetRobot->getRootNode(), VirtualRobot::RobotNodePtr(), true);
    } else
        MMM_ERROR << "Could not initialize ConverterMMM2Robot correctly due to missing joint definitions (target model)" << endl;
    // store markers
    std::vector<SensorPtr> sensors = targetRobot->getSensors();
    for (size_t i = 0; i < sensors.size(); i++)
    {
        targetMarkers[sensors[i]->getName()] = sensors[i];
    }

    if (modelNameDest.empty())
        targetRobot->setName(targetRobot->getType());
    else
        targetRobot->setName(modelNameDest);





	return true;
}

MMM::AbstractMotionPtr ConverterMMM2Robot::convertMotion()
{
    if (!isInitialized())
    {
        return AbstractMotionPtr();
    }
    AbstractMotionPtr res = initializeStepwiseConversion();
    while (res->getNumFrames() < inputMarkerMotion->getNumFrames())
    {
        if (!convertMotionStep(res))
        {
            MMM_ERROR << "Failed to convert motion step " << res->getNumFrames() << endl;
            return res;
        }
		// hack to stop earlier:
		//if (res->getNumFrames() >= 100)
		//	return res;
    }
    return res;
}

bool ConverterMMM2Robot::findBestModelRotation(MMM::LegacyMarkerDataPtr f, int nrRotationsToCheck)
{
	if (nrRotationsToCheck <= 0)
		return false;

	float maxD, avgD;
	getDistance(f, maxD, avgD);

	float bestD = avgD;

	// rotate around z axis
	Eigen::Vector3f z(0.0f, 0.0f, 1.0f);

    Eigen::Matrix4f newGP = targetRobot->getGlobalPose();
    Eigen::Matrix4f startGP = targetRobot->getGlobalPose();
    Eigen::Matrix4f bestGP = targetRobot->getGlobalPose();
	for (int i = 0; i < nrRotationsToCheck; i++)
	{
		float currentStep = (float)M_PI * 2.0f / (float)nrRotationsToCheck * (float)i;
		//cout << "Rot:" << currentStep << endl;
		Eigen::Matrix4f rotMat = MathTools::axisangle2eigen4f(z, currentStep);
		newGP = startGP*rotMat;
        targetRobot->setGlobalPose(newGP);
		getDistance(f, maxD, avgD);
        //cout << "maxD:" << maxD << ",avgD:" << avgD << endl;
		if (avgD < bestD)
		{
			bestD = avgD;
			bestGP = newGP;
		}
	}

	//cout << "Min avg marker distance=" << bestD << endl;
    targetRobot->setGlobalPose(bestGP);
	return true;
}

bool ConverterMMM2Robot::fitModel(MMM::LegacyMarkerDataPtr f, bool quickImprovementCheck, float ikStepSize, float ikMinChange, int ikSteps, bool performMinOneStep, bool boxConstraints)
{
    if (!sourceRobot || !rnsSource || !rnsTarget || !inputMarkerMotion)
		return false;

    //float maxD, avgD;
	//getDistance(f, maxD, avgD);
	//cout << "IK start" << endl;
	//cout << "Dist Avg: " << avgD << endl;
	//cout << "Dist Max: " << maxD << endl;

    // update goals for ik solver
    Eigen::Matrix4f goalPose = Eigen::Matrix4f::Identity();
    float tolRot = float(3.0f/180.0f * M_PI);
	std::map<std::string, std::string>::iterator it = markerMapping.begin();
	while (it != markerMapping.end())
	{
        std::string sourceMarker = it->first;
        std::string targetMarker = it->second;

        if (f->data.find(sourceMarker) == f->data.end())
		{
            MMM_ERROR << "source marker " << sourceMarker << " not present?!" << endl;
			it++;
			continue;
		}
        if (targetMarkers.find(targetMarker) == targetMarkers.end())
		{
            MMM_ERROR << "mmm marker " << targetMarker << " not present?!" << endl;
			it++;
			continue;
		}

		//Eigen::Vector3f posC3D = f->data[c3dMarker];
		//Eigen::Vector3f posMMM = modelMarkers[mmmMarker]->getGlobalPose().block(0, 3, 3, 1);
        goalPose.block(0, 3, 3, 1) = f->data[sourceMarker];
        ik->setGoal(goalPose, targetMarkers[targetMarker], VirtualRobot::IKSolver::Position, 5.0f, tolRot, false);
        it++;
	}

#ifdef _DEBUG
    ik->setVerbose(true);
#endif
    ik->checkImprovements(quickImprovementCheck);
    ik->boxConstraints(boxConstraints);
    ik->computeSteps(ikStepSize, ikMinChange, ikSteps, performMinOneStep);

    _jacobian = ik->getJacobianMatrix();
    //std::cout << "\tJacobian rows/columns: [" << _jacobian.rows() << "|" << _jacobian.cols() << "]" << std::endl;
    //std::cout << "\tRobotNodeSet size : " << rns->getSize() << std::endl;
    //std::cout << "\tMarkerMapping size: " << markerMapping.size() << std::endl;


    float maxD, avgD;
    getDistance(f, maxD, avgD);
    std::cout << "Frame #" << f->frame << " finished: max error = " << maxD << ", avg error = " << avgD << std::endl;

    return true;
}

bool ConverterMMM2Robot::moveModelToCenter(MMM::LegacyMarkerDataPtr frame)
{
	if (!isInitialized() || !frame)
		return false;
    //MMM::LegacyMarkerDataPtr LegacyMarkerData = inputMarkerMotion->getFrame(currentFrame);

	std::map<std::string, Eigen::Vector3f> data = frame->data;
	if (data.size() == 0)
		return false;
	Eigen::Vector3f avgPos(0.0f, 0.0f, 0.0f);
    /*
	for (std::map<std::string, Eigen::Vector3f>::iterator it = data.begin(); it != data.end(); it++)
	{
		avgPos += it->second;
    } 
	avgPos = 1.0f / (float)data.size() * avgPos;
    */

    int nrMarkers = 0;
    std::map<std::string, std::string>::iterator it = markerMapping.begin();
    while (it != markerMapping.end())
    {
        std::string sourceMarker = it->first;

        if (data.find(sourceMarker) != data.end())
        {
            avgPos += data[sourceMarker];
            nrMarkers++;
        }
        it++;
    }
    if (nrMarkers > 0)
        avgPos = 1.0f / (float)nrMarkers * avgPos;
    else
        MMM_WARNING << "Could not find match any marker name of mapping configuration" << endl;
	Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
	globalPose.block(0, 3, 3, 1) = avgPos;
    targetRobot->setGlobalPose(globalPose);
	return true;
}

void ConverterMMM2Robot::getDistance(MMM::LegacyMarkerDataPtr f, float &maxD, float &avgD)
{

	if (!isInitialized() || !f)
	{
		avgD = FLT_MAX;
		maxD = FLT_MAX;
		return;
	}

	// compute distance
	avgD = 0.0f;
	maxD = 0.0f;

	std::map<std::string, std::string>::iterator it = markerMapping.begin();
	while (it != markerMapping.end())
	{
		std::string c3dMarker = it->first;
		std::string mmmMarker = it->second;
		if (f->data.find(c3dMarker) == f->data.end())
		{
            //MMM_ERROR << "c3d marker " << c3dMarker << " not present?!" << endl;
			it++;
			continue;
		}
        if (sourceMarkers.find(mmmMarker) == sourceMarkers.end())
		{
            MMM_ERROR << "mmm marker " << mmmMarker << " not present?!" << endl;
			it++;
			continue;
		}
		Eigen::Vector3f posC3D = f->data[c3dMarker];
        Eigen::Vector3f posMMM = sourceMarkers[mmmMarker]->getGlobalPose().block(0, 3, 3, 1);


		float dist = (posC3D - posMMM).norm();
		//cout << " DIST " << c3dMarker << " <-> " << mmmMarker << ": " << dist << endl;

		if (dist > maxD)
			maxD = dist;

		avgD += dist;

		it++;
	}
	avgD /= markerMapping.size();

	//cout << "Max dist:" << maxD << endl;
	//cout << "Avg Dist:" << avgD << endl;
}

MMM::AbstractMotionPtr ConverterMMM2Robot::initializeStepwiseConversion()
{
    if (!isInitialized())
    {
		MMM_WARNING << "not initialized..." << endl;
        return AbstractMotionPtr();
    }

    // init ik solver
    ik.reset(new RobotPoseDifferentialIK(targetRobot, rnsTarget));
    ik->setVerbose(false);
    float tolRot = float(3.0f/180.0f * M_PI);

    std::map<std::string, std::string>::iterator it = markerMapping.begin();
    Eigen::Matrix4f goalPose = Eigen::Matrix4f::Identity(); // init with standard pose, will be updated later
    // iterate over all markerMapping tuples (1,2) and check for the existence of the mapped target marker (2)
    while (it != markerMapping.end())
    {
        // find the corresponding target marker (2) to the source marker (1)
        std::string targetMarker = it->second;
        // check if this target marker (2) exists
        if (targetMarkers.find(targetMarker) == targetMarkers.end())
        {
            MMM_ERROR << "mmm marker " << targetMarker << " not present?!" << endl;
            it++;
            continue;
        }
        // if it does, set a preliminary goal for this target marker
        ik->setGoal(goalPose, targetMarkers[targetMarker], VirtualRobot::IKSolver::Position, 5.0f, tolRot, false);
        it++;
    }
    // initialize the IK solver
    ik->initialize();

    LegacyMotionPtr res(new LegacyMotion(outputModel->getName()));
    res->setJointOrder(jointOrder);
    MMM::LegacyMarkerDataPtr frame = inputMarkerMotion->getFrame(0);
	moveModelToCenter(frame);
	findBestModelRotation(frame);
    // initially we search harder
    fitModel(frame, paramInitialCheckImprovement, paramInitialIKStepSize, paramInitialIKMinChange,
             paramInitialIKSteps, paramInitialPerfomMinOneStep, paramInitialJointLimitsBoxConstraints);
    initialModelPose = targetRobot->getGlobalPose();
    initialJointValues.clear();
    for (size_t i=0;i<jointOrder.size();i++)
    {
        if (targetRobot->hasRobotNode(jointOrder[i]))
            initialJointValues[jointOrder[i]] = targetRobot->getRobotNode(jointOrder[i])->getJointValue();
        else
            VR_WARNING << "No robot node with name " << jointOrder[i] << endl;
    }

    return res;
}

bool ConverterMMM2Robot::convertMotionStep(AbstractMotionPtr currentOutput)
{
    return convertMotionStep(currentOutput, true);
}

bool ConverterMMM2Robot::convertMotionStep(AbstractMotionPtr currentOutput, bool increment)
{
    if (!isInitialized() || !currentOutput)
    {
        return false;
    }
    if (increment && currentOutput->getNumFrames()>=inputMarkerMotion->getNumFrames())
    {
        MMM_WARNING << "Could not proceed. End of input motion reached..." << endl;
        return false;
    }
    LegacyMotionPtr outputMotion = boost::dynamic_pointer_cast<LegacyMotion>(currentOutput);
    if (!outputMotion)
    {
        MMM_ERROR << "Expecting an outputMotion of type Motion..." << endl;
        return false;
    }


    unsigned int pos = outputMotion->getNumFrames();
    MMM::LegacyMarkerDataPtr frame;

    if (!increment) // for online mapping increment is false
    {
        pos = inputMarkerMotion->getNumFrames()-1;
        frame = inputMarkerMotion->getFrame(pos);
    }
    else
        frame = markerMotion->getFrame(pos);

	if (!frame)
	{
		MMM_ERROR << "Could not grab frame " << pos << " from input marker motion" << endl;
		return false;
    }
    else
        std::cout << "Using frame [" << pos << "] from input motion!" << std::endl;
    int nrDOF = (int)rnsTarget->getSize();

    // TODO: check this
    // convert from MMM-Motion to MarkerPosition and add it to the end of inputMarkerMotion
    //transformPoseToMarker(outputMotion->getMotionFrame(outputMotion->getNumFrames() -1), frame);
    //if (outputMotion->getNumFrames()==0)
    if (outputMotion->getNumFrames()==0 || increment)
    {
        MotionFramePtr mf;
        mf.reset(new MotionFrame(nrDOF));
        outputMotion->addMotionFrame(mf);
    }
    if (!_motion || _motion->getNumFrames()==0)
    {
        MMM_ERROR << "No Motion Frame for conversion available!" << endl;
        return false;
    }


    //transformPoseToMarker(_motion->getMotionFrame(_motion->getNumFrames() -1), frame);

    if (!increment)
        transformPoseToMarker(_motion->getMotionFrame(_motion->getNumFrames() - 1), frame);

    //transformPoseToMarker(_motion->getMotionFrame(outputMotion->getNumFrames() - 1), frame);
    //transformPoseToMarker(outputMotion->getMotionFrame(outputMotion->getNumFrames() -1), frame);

    //markerMotion->getFrame()


    if (!fitModel(frame, paramCheckImprovement, paramIKStepSize, paramIKMinChange, paramIKSteps, paramPerfomMinOneStep, paramJointLimitsBoxConstraints))
	{
		MMM_ERROR << "Error while fitting model..." << endl;
		return false;
	}

	Eigen::VectorXf jv(nrDOF);
    rnsTarget->getJointValues(jv);

    MotionFramePtr mf;
    if (!increment || outputMotion->getNumFrames()==0){
        mf.reset(new MotionFrame(nrDOF));
        outputMotion->addMotionFrame(mf);
    }

    mf=outputMotion->getMotionFrame(outputMotion->getNumFrames()-1);
    mf->joint = jv;
    mf->timestep = frame->timestamp;
    mf->setRootPose(targetRobot->getGlobalPose());
    mf->_customMatrix = _jacobian;

    return true;
}

bool ConverterMMM2Robot::isInitialized()
{
    if (!inputMotion)
        return false;
    if(!outputModel)
        return false;
    if(!inputMarkerMotion)
        return false;
    if(!sourceRobot)
        return false;
    if(!rnsSource)
        return false;
    if(!rnsTarget)
        return false;
    if(inputMarkerMotion->getNumFrames()==0)
        return false;
    if(markerMapping.size() == 0)
        return false;
    return true;
}

bool ConverterMMM2Robot::_setup( rapidxml::xml_node<char>* rootTag )
{
	rapidxml::xml_node<>* node = rootTag;
	std::map<std::string, std::string> markerMappingViconMMM;
    std::string expectedName = name; //ConverterMMM2RobotFactory::getName();
    node = rootTag;//rootTag->first_node();//getChildNodeWithName(expectedName);
    rapidxml::xml_node<>* configNode = rootTag;
	std::string nodeName;
	while (node)
	{

		nodeName = XML::toLowerCase(node->name());
		if (nodeName == "converterconfig" || nodeName == "mapperconfig")
		{
			rapidxml::xml_attribute<> *attr = node->first_attribute("type", 0, false);
			if (!attr)
			{
				MMM_ERROR << "xml Tag: missing attribute 'type'" << endl;
				return false;
			}
			std::string jn = attr->value();
			if (jn.empty())
			{
				MMM_ERROR << "xml tag: null type string" << endl;
				return false;
			}
			XML::toLowerCase(jn);

			XML::toLowerCase(expectedName);
			if (jn != expectedName)
			{
				MMM_ERROR << "xml type tag: Expecting " << expectedName << ", got " << jn << endl;
				return false;
			}
			// found correct node
            configNode = node;
			break;
		}
		else
		{
			// skipping other entries....
			//MMM_ERROR << "Expecting modelprocessorconfig tag, but got " << nodeName << endl;
			//return false;
		}
		node = node->next_sibling();
	}
	if (!node)
	{
		MMM_ERROR << "Could not find modelprocessorconfig tag" << endl;
		return false;
	}
    node = configNode->first_node("model", 0, false);
	if (!node)
	{
		MMM_ERROR << "Could not find model tag" << endl;
		return false;
	}

    mTargetModelSize = XML::getOptionalFloatByAttributeName(node, "size", mTargetModelSize);

	// get first sub node
	node = node->first_node();

	std::vector<std::string> jointList;
	std::string name;
	while (node)
	{
		nodeName = XML::toLowerCase(node->name());
		if (nodeName == "markermapping")
		{
			// retrieve marker mapping
			rapidxml::xml_node<>* subNode = node->first_node();
			while (subNode)
			{
				std::string subNodeName = XML::toLowerCase(subNode->name());
				if (subNodeName == "mapping")
				{
					rapidxml::xml_attribute<> *c3dAttr = subNode->first_attribute("c3d", 0, false);
					rapidxml::xml_attribute<> *mmmAttr = subNode->first_attribute("mmm", 0, false);
					if (!c3dAttr)
					{
						MMM_ERROR << "Missing Attribute C3D in mapping tag " << endl;
					}
					else if (!mmmAttr)
					{
						MMM_ERROR << "Missing Attribute MMM in mapping tag " << endl;
					}
					else
					{
						std::string c3dMarkerName = c3dAttr->value();
						std::string mmmMarkerName = mmmAttr->value();
						if (c3dMarkerName.empty() || mmmMarkerName.empty())
						{
							MMM_ERROR << "Empty marker names not allowed in mapping tag " << endl;
						}
						else
						{
							bool performMapping = true;
							if (markerMappingViconMMM.find(c3dMarkerName) != markerMappingViconMMM.end())
							{
								MMM_ERROR << "C3D marker name " << c3dMarkerName << " is mapped twice!" << endl;
								performMapping = false;
							}
							std::map<std::string, std::string>::iterator i = markerMappingViconMMM.begin();
							while (i != markerMappingViconMMM.end())
							{
								if (i->second == mmmMarkerName)
								{
									MMM_ERROR << "MMM marker name " << mmmMarkerName << " is mapped twice!" << endl;
									performMapping = false;
									break;
								}
								i++;
							}
							if (inputMarkerMotion && !inputMarkerMotion->hasMarkerLabel(c3dMarkerName))
							{
								MMM_ERROR << "Skipping entry: Input c3d marker label not found:" << c3dMarkerName << endl;
								performMapping = false;
							}
							if (outputModel && !outputModel->hasMarker(mmmMarkerName))
							{
								MMM_ERROR << "Skipping entry: Output mmm marker label not found:" << mmmMarkerName << endl;
								performMapping = false;
							}
							if (performMapping)
							{
								markerMappingViconMMM[c3dMarkerName] = mmmMarkerName;
							}
							else
							{
								MMM_ERROR << "Error in marker mapping " << c3dMarkerName << "<->" << mmmMarkerName << "." << endl;
							}
						}
					}
				}
				else
				{
					MMM_ERROR << "Ignoring unknown XML tag " << subNodeName << endl;
				}

				subNode = subNode->next_sibling();
			}
		} else if (nodeName == "jointset")
		{
			rapidxml::xml_node<>* subNode = node->first_node();
			while (subNode)
			{
				std::string subNodeName = XML::toLowerCase(subNode->name());
				if (subNodeName == "joint")
				{
					rapidxml::xml_attribute<> *nameattr = subNode->first_attribute("name", 0, false);

					if (!nameattr)
					{
						MMM_ERROR << "Missing Attribute 'name' in joint tag " << endl;
					}
					else
					{
						std::string jointname = nameattr->value();
						if (!jointname.empty())
							jointList.push_back(jointname);
					}
				}
				else
				{
					MMM_ERROR << "Ignoring unknown XML tag " << subNodeName << endl;
				}
				subNode = subNode->next_sibling();
			}
		} else if (nodeName == "name")
		{
			name = node->value();
		}
		else
		{
			MMM_ERROR << "Ignoring unknown XML tag " << nodeName << endl;
		}
		node = node->next_sibling();
	}

	this->markerMapping = markerMappingViconMMM;
	if (jointList.size() == 0)
	{
		MMM_INFO << "No JointSet tag given in XML config, using all revolute joints" << endl;
		if (inputModel)
		{
			MMM_INFO << "No JointSet given, using all revolute joints" << endl;
			std::vector<ModelNodePtr> models = inputModel->getModels();
			for (size_t i = 0; i < models.size(); i++)
			{
				if (models[i]->joint.jointType == eRevolute && !models[i]->name.empty())
				{
					jointList.push_back(models[i]->name);
				}
			}
		}
	}
    modelNameSrc = name;
	setupJointOrder(jointList);

    // check for IK parameters
    node = configNode->first_node("ik", 0, false);
    if (node)
    {
        // get first sub node
        node = node->first_node();

        std::vector<std::string> jointList;
        std::string name;
        while (node)
        {
            nodeName = XML::toLowerCase(node->name());
            if (nodeName == "initialikparams")
            {
                paramInitialIKSteps = (int)(XML::getOptionalFloatByAttributeName(node, "iksteps", (float)paramInitialIKSteps));
                paramInitialIKMinChange = XML::getOptionalFloatByAttributeName(node, "minchange", paramInitialIKMinChange);
                paramInitialIKStepSize = XML::getOptionalFloatByAttributeName(node, "stepsize", paramInitialIKStepSize);
                paramInitialCheckImprovement = XML::getOptionalBoolByAttributeName(node, "checkimprovement", paramInitialCheckImprovement);
                paramInitialPerfomMinOneStep = XML::getOptionalBoolByAttributeName(node, "minonestep", paramInitialPerfomMinOneStep);
                paramInitialJointLimitsBoxConstraints = XML::getOptionalBoolByAttributeName(node, "boxconstraints", paramInitialJointLimitsBoxConstraints);
            }
            else if (nodeName == "stepikparams")
            {
                paramIKSteps = (int)(XML::getOptionalFloatByAttributeName(node, "iksteps", (float)paramIKSteps));
                paramIKMinChange = XML::getOptionalFloatByAttributeName(node, "minchange", paramIKMinChange);
                paramIKStepSize = XML::getOptionalFloatByAttributeName(node, "stepsize", paramIKStepSize);
                paramCheckImprovement = XML::getOptionalBoolByAttributeName(node, "checkimprovement", paramCheckImprovement);
                paramPerfomMinOneStep = XML::getOptionalBoolByAttributeName(node, "minonestep", paramPerfomMinOneStep);
                paramJointLimitsBoxConstraints = XML::getOptionalBoolByAttributeName(node, "boxconstraints", paramJointLimitsBoxConstraints);
            }
            else
            {
                MMM_ERROR << "Skipping IK tag: Unknown xml tag:" << nodeName << endl;
            }
            node = node->next_sibling();
        }
    }

    return true;
}

}
